/*********************************************************************************************************
  包含头文件
*********************************************************************************************************/
#include "Mouse_Drive.h"
#include "stm32f10x.h"
#include "Zlg7289.h"
#include "Micromouse.h"
#include "Mouse_Config.h"
#include "stmflash.h"
/*********************************************************************************************************
  PA端口定义
*********************************************************************************************************/
#define __D2_1 GPIO_Pin_0 /*   电机 M2 PWMH      */
#define __D2_2 GPIO_Pin_1 /*   电机 M2 PWML      */
#define __D1_1 GPIO_Pin_2 /*   电机 M1 PWMH      */
#define __D1_2 GPIO_Pin_3 /*   电机 M1 PWML      */
#define __KEY GPIO_Pin_11 /* KEY键连接的端口 */
/*********************************************************************************************************
  PB端口定义
*********************************************************************************************************/

/*********************************************************************************************************
  PC端口定义
*********************************************************************************************************/
#define __FRONT_R GPIO_Pin_0     /*  前方右传感器输出的信号        */
#define __FRONTSIDE_R GPIO_Pin_2 /*  45右传感器输出的信号      */
#define __RIGHTSIDE GPIO_Pin_1   /*  右方传感器输出的信号        */
#define __START GPIO_Pin_10      /* START键连接的端口 */
/*********************************************************************************************************
  PD端口定义
*********************************************************************************************************/

/*********************************************************************************************************
  PE端口定义
*********************************************************************************************************/
#define __DIR1 GPIO_Pin_2 /*  编码器 电机 M1 A相      */
#define __DIR2 GPIO_Pin_3 /*  编码器 电机 M2 A相      */

#define __FRONT_L GPIO_Pin_15     /*  前方左传感器输出的信号        */
#define __FRONTSIDE_L GPIO_Pin_13 /*  45左传感器输出的信号      */
#define __LEFTSIDE GPIO_Pin_14    /*  左方传感器输出的信号        */

/*********************************************************************************************************
  常量宏定义--电脑鼠状态
*********************************************************************************************************/
typedef enum _mouse_state
{
  _Stop,         /*  电脑鼠停止                  */
  _GoAhead,      /*  电脑鼠前进                  */
  _TurnLeft,     /*  电脑鼠向左转                */
  _TurnRight,    /*  电脑鼠向右转                */
  _TurnBack,     /*  电脑鼠向后转                */
  _TurnLeftY,    /*  电脑鼠向左转                */
  _TurnRightY,   /*  电脑鼠向右转                */
  _GoBack,       // 后退
  _TurnLeft1,    /*  电脑鼠向左转                */
  _TurnRight1,   /*  电脑鼠向右转                */
  _TurnLeft2,    /*  电脑鼠向左转                */
  _TurnRight2,   /*  电脑鼠向右转                */
  _TurnRightHui, /*  电脑鼠向右转                */
  _TurnLeftHui   /*  电脑鼠向左转                */
} _Mouse_State;
/*********************************************************************************************************
  常量宏定义--电机加减速度
*********************************************************************************************************/
#define __SPEEDUP 0   /*  电机加速                    */
#define __SPEEDDOWN 1 /*  电机减速                */

/*********************************************************************************************************
  定义--电机状态
*********************************************************************************************************/
typedef enum _motor_state
{
  _MotorStop,        /*  电机停止                    */
  _MotorWaitOneStep, /*  电机暂停一步                */
  _MotorRun,         /*  电机运行                    */
  _WaitOneStep1      /*  电机暂停一步                */
} _Motor_State;

/********************************************************************************************************* \
  定义--电机运行方向                                                                        \
*********************************************************************************************************/
typedef enum _motor_dir
{
  _MotorGoAhead, /*  电机前进                    */
  _MotorGoBack,  /*  电机后退                    */
  _MotorGoStop   /*  电机反向制动                */
} _Motor_Dir;

/*********************************************************************************************************
  常量宏定义--PID
*********************************************************************************************************/
#define __KP 80   //80  //比例 30
#define __KI 0.01 //积分 0.01
#define __KD 0    //微分

#define __U_MAX 1400 //返回的最大值,是pwm的周期值
#define __U_MIN 0
#define __ERROR_IMAX 10 //积分限幅
#define __DEAD_BAND 3   //速度PID，设置死区范围

/*********************************************************************************************************
  结构体定义
*********************************************************************************************************/
typedef struct _motor
{
  _Motor_State cState; /*  电机运行状态                */
  _Motor_Dir cRealDir; /*  电机运行方向                */
  uint32 uiPulse;      /*  电机需要运行的脉冲          */
  uint32 uiPulseCtr;   /*  电机已运行的脉冲            */
  int16 sSpeed;        /*  当前占空比                    */
} _Motor;

typedef struct _pid //定义数法核心数据
{
  //uint16 usRef;      //速度PID，速度设定值
  uint16 usFeedBack;     //速度PID，速度反馈值
  uint16 usEncoder_new;  //编码器
  uint16 usEncoder_last; //编码器

  float sRef; // 速度设定值。若是转向速度，左正右负
  int16 sFeedBack;
  float sPreError;  //速度PID，前一次，速度误差,,vi_Ref - vi_FeedBack
  float sPreDerror; //速度PID，前一次，速度误差之差，d_error-PreDerror;

  fp32 fKp; //速度PID，Ka = Kp
  fp32 fKi; //速度PID，Kb = Kp * ( T / Ti )
  fp32 fKd; //速度PID，

  int16 iPreU; //电机控制输出值
} _PID;

/*********************************************************************************************************
  常量宏定义
*********************************************************************************************************/

/*********************************************************************************************************
  定义全局变量
*********************************************************************************************************/
_Map_Coor MouseLocal = {0, 0}; /*  保存电脑鼠当前位置坐标      */
_Map_Dir MouseDir = _North;    /*  保存电脑鼠当前方向          */

/******************************
 * 迷宫地图
 *****************************/
_Map GucMapBlock = {0};
_Map GucMapBlock0 = {0};
_Map GucMapBlock1 = {0x0f};

/*****************************************/
uint8 GucMouseStart = 0; /* 电脑鼠启动        */
uint8 GucFrontJinju = 0; /* 前方红外近距在搜索时等高图制作中用   */
uint8 GucCrossroad = 0;  /* 十字路口数，冲刺时用，若十字路口多降低最高冲刺速度   */

_Motor leftMotor = {_MotorStop, _MotorGoAhead, 0, 0, 0};  /*  定义并初始化左电机状态      */
_Motor rightMotor = {_MotorStop, _MotorGoAhead, 0, 0, 0}; /*  定义并初始化右电机状态      */
_PID leftPID;                                           /*  定义左电机PID      */
_PID rightPID;                                           /*  定义右电机PID     */
_PID speedPID;                                           /*  直线PID     */
_PID rotatePID;                                           /*  旋转PID     */
_Mouse_State mouseState = _Stop;                    /*  保存电脑鼠当前运行状态      */
int32 maxSpeed = SEARCHSPEED;                        /*  保存允许运行的最大速度      */
uint8 AcceleratedSpeed = 0;
uint16 GusFreq_F = 36200;  //33.8,33,327        /*  前方红外频率              */
uint16 GusFreq_FJ = 19200; //26.3,266,275              /*  前方近距红外频率              */
uint16 GusFreq_X = 30000;  //35,33.8          /*  斜45度红外频率              */
uint16 GusFreq_LF = 31700; //34000           /*  左右红外远距频率              */
uint16 GusFreq_L = 18300;  /*  左右红外近距频率              */
uint16 GuiTpusle_LR = 0;
_Bool GucFrontNear = false;
uint8 GucGoHead1 = 0;
uint8 GucGoHead2 = 0;
uint8 GucFangXiang = 0;
uint8 GucDirTemp = 0;
uint16 DIS[10] = {0};
uint8 Tab = 4; //红外调试选项
uint16 count = 0;
extern u16 voltageDetectRef;
extern __IO uint16_t ADC_ConvertedValue[5];
extern float ADC_ConvertedValueLocal[4];
float W;

/*****************
 * 红外传感器读取值
 *****************/
uint16 irValues[4];
uint8 IsWayAhead = 0; // 1 - 前方有路； 0 - 前方无路

//红外的设定值，最后改成数组形式
uint16 DistanceSettings[4][3];

uint16 GusDistance_L_Near = 461; //左红外近
uint16 GusDistance_L_Mid = 384;  //左红外中
uint16 GusDistance_L_Far = 110;  //左红外远

uint16 GusDistance_R_Near = 409;  //右红外近
uint16 GusDistance_R_Mid = 329;   //右红外中
uint16 GusDistance_R_Far = 100;   //右红外近
uint16 GusDistance_FL_Near = 200; //近距，用来判断停止
uint16 GusDistance_FR_Near = 200;
uint16 GusDistance_FL_Far = 110; //远距用来判断墙壁
uint16 GusDistance_FR_Far = 200;
u8 flag = 0;
u8 ir_flag = 0;
u8 mm = 0;
u8 backflag = 0;
u8 backflag2 = 0;
u16 time;

void InitDistanceSettings()
{
  DistanceSettings[_IR_Left][_Near] = __DISTANCE_LEFT_NEAR;
  DistanceSettings[_IR_Left][_Mid] = __DISTANCE_LEFT_MID;                //左红外中
  DistanceSettings[_IR_Left][_Far] = __DISTANCE_LEFT_FAR;                //左红外远
  DistanceSettings[_IR_Right][_Near] = __DISTANCE_RIGHT_NEAR;            //右红外近
  DistanceSettings[_IR_Right][_Mid] = __DISTANCE_RIGHT_MID;              //右红外中
  DistanceSettings[_IR_Right][_Far] = __DISTANCE_RIGHT_FAR;              //右红外近
  DistanceSettings[_IR_FrontLeft][_Near] = __DISTANCE_FRONT_LEFT_NEAR;   //近距，用来判断停止
  DistanceSettings[_IR_FrontRight][_Near] = __DISTANCE_FRONT_RIGHT_NEAR; //
  DistanceSettings[_IR_FrontLeft][_Far] = __DISTANCE_FRONT_LEFT_FAR;     //远距用来判断墙壁
  DistanceSettings[_IR_FrontRight][_Far] = __DISTANCE_FRONT_RIGHT_FAR;   //
}

/*********************************************************************************************************
** Function name:       Delay
** Descriptions:        延时函数
** input parameters:    uiD :延时参数，值越大，延时越久
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void Delay(uint32 uiD)
{
  for (; uiD; uiD--)
    ;
}

void Delay_us(u32 Nus)
{
  SysTick->LOAD = Nus * 9;
  SysTick->CTRL |= 0x01;
  while (!(SysTick->CTRL & (1 << 16)))
    ;
  SysTick->CTRL = 0X00000000;
  SysTick->VAL = 0X00000000;
}

void PIDInit()
{
  leftPID.usEncoder_new = 32768;
  //leftPID.usEncoder_last = 65535;
  leftPID.usFeedBack = 0; //速度反馈值
  leftPID.sFeedBack = 0;

  rightPID.usEncoder_new = 32768;
  //rightPID.usEncoder_last = 65535;
  rightPID.usFeedBack = 0; //速度反馈值
  rightPID.sFeedBack = 0;

  speedPID.sRef = 0; //速度设定值
  speedPID.sFeedBack = 0;
  speedPID.sPreError = 0;  //前一次，速度误差,,vi_Ref - vi_FeedBack
  speedPID.sPreDerror = 0; //前一次，速度误差之差，d_error-PreDerror;

  speedPID.fKp = __KP;
  speedPID.fKi = __KI;
  speedPID.fKd = __KD;

  speedPID.iPreU = 0; //电机控制输出值

  rotatePID.sRef = 0;       //速度设定值
  rotatePID.sFeedBack = 0;  // 转向速度，右正左负
  rotatePID.sPreError = 0;  //前一次，速度误差,,vi_Ref - vi_FeedBack
  rotatePID.sPreDerror = 0; //前一次，速度误差之差，d_error-PreDerror;

  rotatePID.fKp = __KP; //30
  rotatePID.fKi = __KI; //0.1,0.01
  rotatePID.fKd = __KD;

  rotatePID.iPreU = 0; //电机控制输出值
}

/*********************************************************************************************************
** Function name:       __PIDCalculate
** Descriptions:        PID计算
** input parameters:    pid - 计算前的PID
** output parameters:   pid - 计算后的PID
** Returned value:      无
*********************************************************************************************************/
void __PIDCalculate(_PID *pid)
{
  float error, d_error, dd_error;
  uint8 k_i; // 是否计算积分部

  error = pid->sRef - pid->sFeedBack;   // 偏差计算 = e
  d_error = error - pid->sPreError;     // = e - e1
  dd_error = d_error - pid->sPreDerror; // = (e-e1) - (e1-e2) = e - 2*e1 + e2

  // 排除死区
  if (error > __DEAD_BAND)
    error -= __DEAD_BAND;
  else if (error < -__DEAD_BAND)
    error += __DEAD_BAND;
  else
    error = 0; // 在死区范围内

  // 是否计算积分部
  k_i = (error > __ERROR_IMAX) || (error < -__ERROR_IMAX) ? 0 : 1;

  pid->sPreError = error; //存储当前偏差
  pid->sPreDerror = d_error;

  /*
   * 累计 ΔU, 得到:
   *   0 + ... + ΔU2 + ΔU1 + ΔU
   * = 0 + ... + (U2-U3) + (U1-U2) + (U-U1)
   * = 0 + ... + U
   * = U
   */
  pid->iPreU += (int16)(pid->fKp * d_error + k_i * pid->fKi * error + pid->fKd * dd_error);
}

/*********************************************************************************************************
** Function name:       __RightMotorControl
** Descriptions:        右直流电机驱动
** input parameters:    speed: 电机速度控制
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __RightMotorControl(int16 speed)
{
  if (speed >= 0)
  {
    TIM_SetCompare1(TIM1, speed);
    TIM_SetCompare2(TIM1, 0);
  }
  else
  {
    TIM_SetCompare1(TIM1, 0);
    TIM_SetCompare2(TIM1, -speed);
  }
}

/*********************************************************************************************************
** Function name:       __LeftMotorControl
** Descriptions:        左直流电机驱动
** input parameters:    speed: 电机速度控制
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __LeftMotorControl(int16 speed)
{
  if (speed >= 0)
  {
    TIM_SetCompare4(TIM1, 0);
    TIM_SetCompare3(TIM1, speed);
  }
  else
  {
    TIM_SetCompare4(TIM1, -speed);
    TIM_SetCompare3(TIM1, 0);
  }
}

/*********************************************************
 * 获取安全范围内的速度PID
 * 输入：speed - 输入的速度PID
 * 返回：安全范围内的速度PID
*********************************************************/
int16 __SafeSpeed(int16 speed)
{
  int16 sign;
  sign = speed >= 0 ? 1 : -1;

  if (speed * sign > __U_MAX) //速度PID，防止调节最高溢出
    speed = __U_MAX * sign;

  if (speed * sign < __U_MIN) //速度PID，防止调节最低溢出
    speed = __U_MIN * sign;

  return speed;
}

/*********************************************************************************************************
** Function name:      __PIDControl
** Descriptions:        PID控制，通过脉冲数控制电机
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __PIDControl()
{
  __PIDCalculate(&speedPID);
  __PIDCalculate(&rotatePID);

  leftMotor.sSpeed = (leftMotor.cState == _MotorStop) ? 0 : __SafeSpeed(speedPID.iPreU - rotatePID.iPreU);

  rightMotor.sSpeed = (rightMotor.cState == _MotorStop) ? 0 : __SafeSpeed(speedPID.iPreU + rotatePID.iPreU);

  __RightMotorControl(rightMotor.sSpeed);
  __LeftMotorControl(leftMotor.sSpeed);
}

/*********************************************************************************************************
** Function name:       __Encoder
** Descriptions:        采集编码器输出的脉冲
** input parameters:    无
** output parameters:   无
** Returned value:      无
**
*********************************************************************************************************/
void __Encoder(void)
{
  static u16 Dir_L;
  static u16 Dir_R;

  leftPID.usEncoder_new = TIM_GetCounter(TIM2);
  rightPID.usEncoder_new = TIM_GetCounter(TIM3);

  Dir_R = TIM3->CR1;
  Dir_R = (Dir_R & 0x0010) >> 4;

  Dir_L = TIM2->CR1;
  Dir_L = (Dir_L & 0x0010) >> 4;

  if (Dir_L == 1) //向下计数  向后退
  {
    leftPID.usFeedBack = 32768 - leftPID.usEncoder_new;
    if (leftPID.usFeedBack > 400)
      leftPID.usFeedBack = 350;
    //leftPID.usEncoder_last = leftPID.usEncoder_new;
    TIM_SetCounter(TIM2, 32768);
    leftMotor.uiPulseCtr += leftPID.usFeedBack;
    leftMotor.cRealDir = _MotorGoBack;
    leftPID.sFeedBack = -1 * leftPID.usFeedBack;
  }
  else
  {
    leftPID.usFeedBack = leftPID.usEncoder_new - 32768;
    TIM_SetCounter(TIM2, 32768);
    if (leftPID.usFeedBack > 400)
      leftPID.usFeedBack = 350;
    leftMotor.uiPulseCtr += leftPID.usFeedBack;
    leftMotor.cRealDir = _MotorGoAhead;
    leftPID.sFeedBack = leftPID.usFeedBack;
  }

  if (Dir_R == 1) //向下计数  前进
  {
    rightPID.usFeedBack = 32768 - rightPID.usEncoder_new;
    if (rightPID.usFeedBack > 400)
      rightPID.usFeedBack = 350;
    TIM_SetCounter(TIM3, 32768);
    rightMotor.uiPulseCtr += rightPID.usFeedBack;
    rightMotor.cRealDir = _MotorGoAhead;
    rightPID.sFeedBack = rightPID.usFeedBack;
  }
  else
  {
    rightPID.usFeedBack = rightPID.usEncoder_new - 32768;
    TIM_SetCounter(TIM3, 32768);
    if (rightPID.usFeedBack > 400)
      rightPID.usFeedBack = 350;
    rightMotor.uiPulseCtr += rightPID.usFeedBack;
    rightMotor.cRealDir = _MotorGoBack;
    rightPID.sFeedBack = -1 * rightPID.usFeedBack;
  }

  speedPID.sFeedBack = (rightPID.sFeedBack + leftPID.sFeedBack) / 2;
  rotatePID.sFeedBack = (rightPID.sFeedBack - leftPID.sFeedBack) / 2;
}

_Bool within(_IR ir, _Distance distanceIndex)
{
  return ir == _IR_Front
             ? within(_IR_FrontRight, distanceIndex) && within(_IR_FrontLeft, distanceIndex)
             : irValues[ir] >= DistanceSettings[ir][distanceIndex];
}

uint8 GetDistanceStatus(_IR ir)
{
  uint8 result = within(ir, _Near) ? 1 : 0;

  if (ir == _IR_Right || ir == _IR_Left)
  {
    result <<= 1;
    result |= within(ir, _Mid) ? 1 : 0;
  }

  result <<= 1;
  result |= within(ir, _Far) ? 1 : 0;

  return result;
}

void __callAtIrIO(int8 ir, void (*func)(GPIO_TypeDef *, uint16_t))
{
  switch (ir)
  {
  case _IR_FrontLeft:
    (*func)(GPIOA, GPIO_Pin_5); //前方左 开
    break;

  case _IR_FrontRight:
    (*func)(GPIOC, GPIO_Pin_13); //前方右 开
    break;

  case _IR_Left:
    (*func)(GPIOA, GPIO_Pin_3); //左 开
    break;

  case _IR_Right:
    (*func)(GPIOC, GPIO_Pin_2); //右 开
    break;

  default:
    break;
  }
}

void __irReset(int8 ir)
{
  __callAtIrIO(ir, GPIO_ResetBits);
}

void __irSendFreq(int8 ir)
{
  __callAtIrIO(ir, GPIO_SetBits);
}

void makeIrSafe(uint8 ir)
{
  irValues[ir] = irValues[ir] < 0 || irValues[ir] > (ir == _IR_Right ? 35000 : 30000) ? 0 : irValues[ir];
}

void __irCheck()
{
  static uint16 sunlight = 0;
  static uint8 ir = 0;

  irValues[ir] = ADC_ConvertedValue[ir] - sunlight;

  makeIrSafe(ir);

  __irReset(ir);

  ir++;
  ir %= 4;
  sunlight = 0;
  Delay(1000);
  sunlight = ADC_ConvertedValue[ir];
  __irSendFreq(ir);
}

void SensorDebug(void)
{
  zlg7289Download(2, 0, 0, GetDistanceStatus(_IR_Left));       //传感器左
  zlg7289Download(2, 1, 0, GetDistanceStatus(_IR_FrontLeft));  //传感器前左
  zlg7289Download(2, 2, 0, GetDistanceStatus(_IR_FrontRight)); //传感器前右
  zlg7289Download(2, 3, 0, GetDistanceStatus(_IR_Right));      //传感器右
}

/*********************************************************************************************************
** Function name:       __SpeedUp
** Descriptions:        电脑鼠加速程序
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __SpeedUp(void)
{
  if (maxSpeed > 0)
  {
    if (speedPID.sFeedBack >= speedPID.sRef)
      speedPID.sRef += AcceleratedSpeed;

    if (speedPID.sRef > maxSpeed)
      speedPID.sRef = maxSpeed;

    if (speedPID.sRef < 0)
      speedPID.sRef = 0;
  }

  if (maxSpeed < 0)
  {
    if (speedPID.sFeedBack <= speedPID.sRef)
      speedPID.sRef -= AcceleratedSpeed;

    if (speedPID.sRef < maxSpeed)
      speedPID.sRef = maxSpeed;

    if (speedPID.sRef > 0)
      speedPID.sRef = 0;
  }
}

/*********************************************************************************************************
** Function name:       StartCheck
** Descriptions:        读取按键
** input parameters:    无
** output parameters:   无
** Returned value:      true:  按键已按下
**                      false: 按键未按下
*********************************************************************************************************/
uint8 StartCheck(void)
{
  if (GPIO_ReadInputDataBit(GPIOC, __START) == 0)
  {
    Delay(50);
    while (GPIO_ReadInputDataBit(GPIOC, __START) == 0)
      ;
    return (true);
  }
  else
  {
    return (false);
  }
}

void TIM2_IROHandler(void)
{
  TIM_ClearFlag(TIM2, TIM_FLAG_Update);
}

void TIM3_IRQHandler(void)
{
  TIM_ClearFlag(TIM3, TIM_FLAG_Update);
}

/*********************************************************************************************************
** Function name:       SysTick_Handler
** Descriptions:        定时中断扫描。
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void SysTick_Handler(void)
{
  __Encoder();
  __irCheck();

  switch (mouseState)
  {
  case _GoAhead: /*  根据传感器状态微调电机位置  */

    if (within(_IR_Left, _Near) && !within(_IR_Right, _Near))
    {
      // 左的近距内，右的近距外
      rotatePID.sRef -= 1;
    }
    else if (!within(_IR_Left, _Near) && within(_IR_Right, _Near))
    {
      // 右的近距内，左的近距外
      rotatePID.sRef += 1;
    }

    else if (!within(_IR_Left, _Mid) && within(_IR_Left, _Far) && !within(_IR_Right, _Far))
    {
      // 左的中距与远距之间，右的远距外
      rotatePID.sRef -= 1.25;
    }
    else if (!within(_IR_Right, _Mid) && within(_IR_Right, _Far) && !within(_IR_Left, _Far))
    {
      // 右的中距与远距之间，左的远距外
      rotatePID.sRef += 1.25;
    }
    else if (within(_IR_Left, _Near) && !within(_IR_Right, _Mid))
    {
      // 左的近距内，右的中距外
      rotatePID.sRef -= 1.5;
    }
    else if (within(_IR_Right, _Near) && !within(_IR_Left, _Mid))
    {
      // 右的近距内，左的中距外
      rotatePID.sRef += 1.5;
    }
    else
    {
      rotatePID.sRef = 0;
    }

    __SpeedUp();
    break;

  case _TurnRight:
    if (time > 0)
    {
      rotatePID.sRef = -50;
    }
    time++;
    break;

  case _TurnLeft:  //左转
    if (time >= 0) //163
    {
      rotatePID.sRef = 50;
    }
    time++;
    break;

  case _TurnRight1:
    if (time > 0)
    {
      rotatePID.sRef = -60; // 60
    }
    time++;
    break;

  case _TurnBack: /////
    if (backflag == 1)
    {
      backflag = 0;
      if (time >= 0)
        rotatePID.sRef = -40;

      time++;
    }

    else
    {
      if (time >= 0)
        rotatePID.sRef = -40;
      time++;
    }
    break;

  default:
    break;
  }
  __PIDControl();
}

/*********************************************************************************************************
** Function name:       MazeSearch
** Descriptions:        前进N格
** input parameters:    iNblock: 前进的格数
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/

void MouseGoahead_Hui(int8 cNBlock) //电脑鼠原路返回时程序
{
  int8 cL = 0, cR = 0, cCoor = 1;
  if (leftMotor.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
    }
    else
    {
      GuiTpusle_LR = 10000;
    }
    maxSpeed = 110;
  }
  else
  {
    GuiTpusle_LR = 0;
    maxSpeed = 250;
  }
  GucFangXiang = GucDirTemp;
  if (((MouseLocal.x == 7) && (MouseLocal.y == 7)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 7) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
  }
  if ((mouseState == _TurnRight) || (mouseState == _TurnLeft))
  {
    leftMotor.uiPulseCtr = 30000; //1182(34mm)
    rightMotor.uiPulseCtr = 30000;
  }

  mouseState = _GoAhead;

  rightMotor.uiPulse = rightMotor.uiPulse + cNBlock * ONEBLOCK;
  leftMotor.uiPulse = leftMotor.uiPulse + cNBlock * ONEBLOCK;
  rightMotor.cState = _MotorRun;
  leftMotor.cState = _MotorRun;
  if (cNBlock > 7)
  {
    maxSpeed = 250; //300//320
  }

  else if (cNBlock == 7)
  {
    maxSpeed = 240; //250//270
  }
  else if (cNBlock == 6)
  {
    maxSpeed = 220; //250//270
  }

  else if (cNBlock == 5)
  {
    maxSpeed = 200; //250//270
  }
  else if (cNBlock == 4)
  {
    maxSpeed = 160; //200//220
  }
  else if (cNBlock == 3)
  {
    maxSpeed = 120; //180//200
  }
  else if (cNBlock == 2)
  {
    maxSpeed = 110; //150//160
  }
  else
    ;

  while (leftMotor.cState != _MotorStop)
  {

    if (leftMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      leftMotor.uiPulse -= ONEBLOCK;
      leftMotor.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          return;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (rightMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      rightMotor.uiPulse -= ONEBLOCK;
      rightMotor.uiPulseCtr -= ONEBLOCK;
    }
    if (cNBlock < 2)
    {

      if (speedPID.sFeedBack > 100)
      {
        speedPID.sRef = 100;
      }

      if (cL)
      {                              /*  是否允许检测左边            */
        if (!within(_IR_Left, _Far)) /*  左边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 19000 - GuiTpusle_LR; //电脑鼠返回时左转之前20100
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 19000 - GuiTpusle_LR;   //电脑鼠返回时左转之前20100
                                                                           // MouseStop();
                                                                           //while(1);

          while (!within(_IR_Left, _Far))
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (within(_IR_Left, _Far))
        {
          cL = 1;
        }
      }

      if (cR)
      {                               /*  是否允许检测右边            */
        if (!within(_IR_Right, _Far)) /*  右边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 19000 - GuiTpusle_LR; //电脑鼠返回时右转之前20200
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 19000 - GuiTpusle_LR;   //电脑鼠返回时右转之前20200
          //MouseStop();
          // while(1);

          while (!within(_IR_Right, _Far))
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      {
        if (within(_IR_Right, _Far))
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
}

void MouseGoahead_Hui_Low(int8 cNBlock) //电脑鼠搜索返回连续转弯用
{
  int8 cL = 0, cR = 0, cCoor = 1;
  if (leftMotor.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;
    //GuiTpusle_LR = 0;    //100
    if (GucFangXiang == GucDirTemp)
    {
      //GucYiBaiBa=1;
      GuiTpusle_LR = 15000;
    }
    else
    {
      GuiTpusle_LR = 10000;
    }
    maxSpeed = 100;
  }
  else
  {
    GuiTpusle_LR = 0;
    maxSpeed = 100;
  }
  GucFangXiang = GucDirTemp;
  if (((MouseLocal.x == 7) && (MouseLocal.y == 7)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 7) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
  }
  if ((mouseState == _TurnRight) || (mouseState == _TurnLeft))
  {
    leftMotor.uiPulseCtr = 30000; //1182(34mm)
    rightMotor.uiPulseCtr = 30000;
  }

  mouseState = _GoAhead;

  rightMotor.uiPulse = rightMotor.uiPulse + cNBlock * ONEBLOCK;
  leftMotor.uiPulse = leftMotor.uiPulse + cNBlock * ONEBLOCK;
  rightMotor.cState = _MotorRun;
  leftMotor.cState = _MotorRun;

  while (leftMotor.cState != _MotorStop)
  {

    if (leftMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      leftMotor.uiPulse -= ONEBLOCK;
      leftMotor.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          return;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (rightMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      rightMotor.uiPulse -= ONEBLOCK;
      rightMotor.uiPulseCtr -= ONEBLOCK;
    }
    if (cNBlock < 2)
    {

      if (speedPID.sFeedBack > 100)
      {
        speedPID.sRef = 100;
      }

      if (cL)
      {                              /*  是否允许检测左边            */
        if (!within(_IR_Left, _Far)) /*  左边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 15200 - GuiTpusle_LR; //3094(89mm)22000
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 15200 - GuiTpusle_LR;   //22000
                                                                           // MouseStop();
                                                                           //while(1);

          while (!within(_IR_Left, _Far))
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (within(_IR_Left, _Far))
        {
          cL = 1;
        }
      }
      if (cR)
      {                               /*  是否允许检测右边            */
        if (!within(_IR_Right, _Far)) /*  右边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 15000 - GuiTpusle_LR; //22000
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 15000 - GuiTpusle_LR;   //22000

          //MouseStop();
          //while(1);

          while (!within(_IR_Right, _Far))
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      {
        if (within(_IR_Right, _Far))
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
}

_Map_Dir TurnDir(_Map_Dir mapDir, _Relative_Dir relativeDir)
{
  return (_Map_Dir)(((int)mapDir + (int)relativeDir) % 4);
}

void MouseTurnRight_Hui(void) //电脑鼠搜索返回右转
{

  time = 0;

  rotatePID.sRef = -50; //转弯角速度
  speedPID.sRef = 100; //转弯直线速度

  mouseState = _TurnRight;

  rightMotor.cState = _MotorRun; //得加电脑鼠状态
  leftMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Right); /*  方向标记                    */

  while (1)
  {
    if (time > 300) //290
      break;
  }

  rotatePID.sRef = 0;
  mouseState = _GoAhead;
  leftMotor.uiPulse = 5200; //转弯之后距离，调节连续转弯用  5300
  leftMotor.uiPulseCtr = 0;
  rightMotor.uiPulse = 5200; //转弯之后距离，调节连续转弯用     5300
  rightMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  mouseState = _TurnRight;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 10000;
  leftMotor.uiPulseCtr = 10000;
  // MouseStop();
  // while(1);
}
void MouseTurnLeft_Hui(void) //电脑鼠搜索返回左转
{

  time = 0;
  rotatePID.sRef = 50;  //转弯角速度
  speedPID.sRef = 100; //转弯直线速度
  mouseState = _TurnLeft;
  rightMotor.cState = _MotorRun; //得加电脑鼠状态
  leftMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Left); /*  方向标记                    */
  while (1)
  {
    if (time > 300) //292
      break;
  }
  rotatePID.sRef = 0;
  mouseState = _GoAhead;

  //MouseStop();
  //while(1);
  leftMotor.uiPulse = 6000; //转弯之后距离，调节连续转弯用  4900
  leftMotor.uiPulseCtr = 0;
  rightMotor.uiPulse = 6000; //转弯之后距离，调节连续转弯用     4900
  rightMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  mouseState = _TurnLeft;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 10000;
  leftMotor.uiPulseCtr = 10000;
  //MouseStop();
  //while(1);
}

void MouseGoahead1(int8 cNBlock) //冲刺看前方红外，无坐标更新,当goto end速度大于20时设定速度为20
{
  int8 cL = 0, cR = 0, cCoor = 1;
  if (leftMotor.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
    }
    else
    {
      GuiTpusle_LR = 10000;
    }
    maxSpeed = 150;
  }
  else
  {
    GuiTpusle_LR = 0;
    maxSpeed = 350;
  }
  GucFangXiang = GucDirTemp;
  if (((MouseLocal.x == 7) && (MouseLocal.y == 7)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 7) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
  }
  if ((mouseState == _TurnRight) || (mouseState == _TurnLeft))
  {
    leftMotor.uiPulseCtr = 30000; //1182(34mm)
    rightMotor.uiPulseCtr = 30000;
  }

  mouseState = _GoAhead;

  rightMotor.uiPulse = rightMotor.uiPulse + cNBlock * ONEBLOCK;
  leftMotor.uiPulse = leftMotor.uiPulse + cNBlock * ONEBLOCK;
  rightMotor.cState = _MotorRun;
  leftMotor.cState = _MotorRun;
  if (cNBlock > 7)
  {
    maxSpeed = 350; //300//320
  }

  else if (cNBlock == 7)
  {
    maxSpeed = 350; //250//270
  }
  else if (cNBlock == 6)
  {
    maxSpeed = 330; //250//270
  }

  else if (cNBlock == 5)
  {
    maxSpeed = 300; //250//270
  }
  else if (cNBlock == 4)
  {
    maxSpeed = 240; //200//220
  }
  else if (cNBlock == 3)
  {
    maxSpeed = 180; //180//200
  }
  else if (cNBlock == 2)
  {
    maxSpeed = 150; //150//160
  }
  else
    ;

  while (leftMotor.cState != _MotorStop)
  {

    if (leftMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      leftMotor.uiPulse -= ONEBLOCK;
      leftMotor.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          return;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (rightMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      rightMotor.uiPulse -= ONEBLOCK;
      rightMotor.uiPulseCtr -= ONEBLOCK;
    }
    if (cNBlock < 2)
    {

      if (speedPID.sFeedBack > 120)
      {
        speedPID.sRef = 120;
      }

      if (cL)
      {                              /*  是否允许检测左边            */
        if (!within(_IR_Left, _Far)) /*  左边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 17000 - GuiTpusle_LR; //不动18000
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 17000 - GuiTpusle_LR;   //
          while (!within(_IR_Left, _Far))

          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (within(_IR_Left, _Far))
        {
          cL = 1;
        }
      }
      if (cR)
      {                               /*  是否允许检测右边            */
        if (!within(_IR_Right, _Far)) /*  右边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 17000 - GuiTpusle_LR; //15000不动
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 17000 - GuiTpusle_LR;   //15000
          while (!within(_IR_Right, _Far))
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      {
        if (within(_IR_Right, _Far))
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
}

void MouseGoahead1_Low(int8 cNBlock) //连续转弯用
{
  int8 cL = 0, cR = 0, cCoor = 1;
  if (leftMotor.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
    }
    else
    {
      GuiTpusle_LR = 10000;
    }
    maxSpeed = 120;
  }
  else
  {
    GuiTpusle_LR = 0;
    maxSpeed = 120;
  }
  GucFangXiang = GucDirTemp;
  if (((MouseLocal.x == 7) && (MouseLocal.y == 7)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 7) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
  }
  if ((mouseState == _TurnRight) || (mouseState == _TurnLeft))
  {
    leftMotor.uiPulseCtr = 30000; //1182(34mm)
    rightMotor.uiPulseCtr = 30000;
  }

  mouseState = _GoAhead;

  rightMotor.uiPulse = rightMotor.uiPulse + cNBlock * ONEBLOCK;
  leftMotor.uiPulse = leftMotor.uiPulse + cNBlock * ONEBLOCK;
  rightMotor.cState = _MotorRun;
  leftMotor.cState = _MotorRun;

  while (leftMotor.cState != _MotorStop)
  {

    if (leftMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      leftMotor.uiPulse -= ONEBLOCK;
      leftMotor.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          return;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (rightMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      rightMotor.uiPulse -= ONEBLOCK;
      rightMotor.uiPulseCtr -= ONEBLOCK;
    }

    if (cNBlock < 2)
    {

      if (speedPID.sFeedBack > 120)
      {
        speedPID.sRef = 120;
      }

      if (cL)
      {                                             /*  是否允许检测左边            */
        if (irValues[_IR_Left] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 13000 - GuiTpusle_LR; //不动18000
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 13000 - GuiTpusle_LR;   //
                                                                           // MouseStop();
                                                                           // while(1);

          while (irValues[_IR_Left] < GusDistance_L_Far)
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (within(_IR_Left, _Far))
        {
          cL = 1;
        }
      }
      if (cR)
      {                                              /*  是否允许检测右边            */
        if (irValues[_IR_Right] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 9000 - GuiTpusle_LR; //不动18500
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 9000 - GuiTpusle_LR;

          while (irValues[_IR_Right] < GusDistance_R_Far)
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      {
        if (within(_IR_Right, _Far))
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
}

//以下是二次冲刺
void MouseGoahead2(int8 cNBlock) //冲刺看前方红外，无坐标更新,当goto end速度大于20时设定速度为20
{
  int8 cL = 0, cR = 0, cCoor = 1;
  if (leftMotor.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
    }
    else
    {
      GuiTpusle_LR = 10000;
    }
    maxSpeed = 160;
  }
  else
  {
    GuiTpusle_LR = 0;
    maxSpeed = 350;
  }
  GucFangXiang = GucDirTemp;
  if (((MouseLocal.x == 7) && (MouseLocal.y == 7)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 7) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
  }
  if ((mouseState == _TurnRight) || (mouseState == _TurnLeft))
  {
    leftMotor.uiPulseCtr = 30000; //1182(34mm)
    rightMotor.uiPulseCtr = 30000;
  }

  mouseState = _GoAhead;

  rightMotor.uiPulse = rightMotor.uiPulse + cNBlock * ONEBLOCK;
  leftMotor.uiPulse = leftMotor.uiPulse + cNBlock * ONEBLOCK;
  rightMotor.cState = _MotorRun;
  leftMotor.cState = _MotorRun;
  if (cNBlock > 7)
  {
    maxSpeed = 350; //300//320
  }

  else if (cNBlock == 7)
  {
    maxSpeed = 350; //250//270
  }
  else if (cNBlock == 6)
  {
    maxSpeed = 330; //250//270
  }

  else if (cNBlock == 5)
  {
    maxSpeed = 320; //250//270
  }
  else if (cNBlock == 4)
  {
    maxSpeed = 320; //200//220
  }
  else if (cNBlock == 3)
  {
    maxSpeed = 240; //180//200
  }
  else if (cNBlock == 2)
  {
    maxSpeed = 160; //150//160
  }
  else
    ;

  while (leftMotor.cState != _MotorStop)
  {

    if (leftMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      leftMotor.uiPulse -= ONEBLOCK;
      leftMotor.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          return;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (rightMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      rightMotor.uiPulse -= ONEBLOCK;
      rightMotor.uiPulseCtr -= ONEBLOCK;
    }
    if (cNBlock < 2)
    {

      if (speedPID.sFeedBack > 120)
      {
        speedPID.sRef = 120;
      }

      if (cL)
      {                                             /*  是否允许检测左边            */
        if (irValues[_IR_Left] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 17000 - GuiTpusle_LR; //不动18000
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 17000 - GuiTpusle_LR;   //
          while (irValues[_IR_Left] < GusDistance_L_Far)

          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (within(_IR_Left, _Far))
        {
          cL = 1;
        }
      }
      if (cR)
      {                                              /*  是否允许检测右边            */
        if (irValues[_IR_Right] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 17000 - GuiTpusle_LR; //15000不动
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 17000 - GuiTpusle_LR;   //15000
          while (irValues[_IR_Right] < GusDistance_R_Far)
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      {
        if (within(_IR_Right, _Far))
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
}

void MouseGoahead2_Low(int8 cNBlock) //连续转弯用
{
  int8 cL = 0, cR = 0, cCoor = 1;
  if (leftMotor.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
    }
    else
    {
      GuiTpusle_LR = 10000;
    }
    maxSpeed = 120;
  }
  else
  {
    GuiTpusle_LR = 0;
    maxSpeed = 120;
  }
  GucFangXiang = GucDirTemp;
  if (((MouseLocal.x == 7) && (MouseLocal.y == 7)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 7) && (MouseLocal.y == 8)) ||
      ((MouseLocal.x == 8) && (MouseLocal.y == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
  }
  if ((mouseState == _TurnRight) || (mouseState == _TurnLeft))
  {
    leftMotor.uiPulseCtr = 30000; //1182(34mm)
    rightMotor.uiPulseCtr = 30000;
  }

  mouseState = _GoAhead;

  rightMotor.uiPulse = rightMotor.uiPulse + cNBlock * ONEBLOCK;
  leftMotor.uiPulse = leftMotor.uiPulse + cNBlock * ONEBLOCK;
  rightMotor.cState = _MotorRun;
  leftMotor.cState = _MotorRun;

  while (leftMotor.cState != _MotorStop)
  {

    if (leftMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      leftMotor.uiPulse -= ONEBLOCK;
      leftMotor.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          return;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (rightMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      rightMotor.uiPulse -= ONEBLOCK;
      rightMotor.uiPulseCtr -= ONEBLOCK;
    }

    if (cNBlock < 2)
    {

      if (speedPID.sFeedBack > 120)
      {
        speedPID.sRef = 120;
      }

      if (cL)
      {                                             /*  是否允许检测左边            */
        if (irValues[_IR_Left] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 13000 - GuiTpusle_LR; //不动18000
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 13000 - GuiTpusle_LR;   //
                                                                           // MouseStop();
                                                                           // while(1);

          while (irValues[_IR_Left] < GusDistance_L_Far)
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (within(_IR_Left, _Far))
        {
          cL = 1;
        }
      }
      if (cR)
      {                                              /*  是否允许检测右边            */
        if (irValues[_IR_Right] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
        {
          rightMotor.uiPulse = rightMotor.uiPulseCtr + 9000 - GuiTpusle_LR; //不动18500
          leftMotor.uiPulse = leftMotor.uiPulseCtr + 9000 - GuiTpusle_LR;

          while (irValues[_IR_Right] < GusDistance_R_Far)
          {

            if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
            {
              return;
            }
          }
        }
      }
      else
      {
        if (within(_IR_Right, _Far))
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
}

void MouseTurnRight1(void) //第一次冲刺右转用
{

  time = 0;
  rotatePID.sRef = -55; //转弯角速度
  speedPID.sRef = 110; //转弯直线速度

  mouseState = _TurnRight;

  rightMotor.cState = _MotorRun; //得加电脑鼠状态
  leftMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Right); /*  方向标记                    */
  while (1)
  {
    if (time > 280) //转弯角度调节290
      break;
  }

  rotatePID.sRef = 0;
  mouseState = _GoAhead;
  // MouseStop();
  // while(1);

  leftMotor.uiPulse = 4000; //转弯之后，连续转弯调节用      5000  6300
  leftMotor.uiPulseCtr = 0;
  rightMotor.uiPulse = 4000; //转弯之后，连续转弯调节用        5000  不动
  rightMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  mouseState = _TurnRight;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 10000;
  leftMotor.uiPulseCtr = 10000;
  //MouseStop();
  //while(1);
}

void MouseTurnLeft1(void) //第一次冲刺左转用
{

  time = 0;
  rotatePID.sRef = 55;  //转弯角速度
  speedPID.sRef = 110; //转弯直线速度
  mouseState = _TurnLeft;
  rightMotor.cState = _MotorRun; //得加电脑鼠状态
  leftMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Left); /*  方向标记                    */
  while (1)
  {
    if (time > 290) //转弯角度调节259
      break;
  }
  rotatePID.sRef = 0;
  mouseState = _GoAhead;

  // MouseStop();
  //while(1);
  leftMotor.uiPulse = 4000; //转弯之后，调节连续转弯用   11500
  leftMotor.uiPulseCtr = 0;
  rightMotor.uiPulse = 4000; //转弯之后，调节连续转弯用   11500
  rightMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  mouseState = _TurnLeft;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 10000;
  leftMotor.uiPulseCtr = 10000;
  //MouseStop();
  //while(1);
}

void mouseTurnright2(void) //电脑鼠第二次冲刺右转用
{
  time = 0;
  rotatePID.sRef = -60; //18
  speedPID.sRef = 115; //18

  mouseState = _TurnRight;

  rightMotor.cState = _MotorRun; //得加电脑鼠状态
  leftMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Right); /*  方向标记                    */
                                        /* mouseState   = GoAhead;//
   MouseStop();
   while(1); */
  while (1)
  {
    if (time > 283)
      break;
  }

  rotatePID.sRef = 0;
  mouseState = _GoAhead;
  //MouseStop();
  //while(1);

  //MouseStop();
  //while(1);
  leftMotor.uiPulse = 3050; //9000
  leftMotor.uiPulseCtr = 0;
  rightMotor.uiPulse = 3050;
  rightMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  mouseState = _TurnRight;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 10000;
  leftMotor.uiPulseCtr = 10000;
  // MouseStop();
  //while(1);
}

void mouseTurnleft2(void) //电脑鼠第二次冲刺左转用
{

  time = 0;
  rotatePID.sRef = 60;  //18
  speedPID.sRef = 115; //18
  mouseState = _TurnLeft;
  rightMotor.cState = _MotorRun; //得加电脑鼠状态
  leftMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Left); /*  方向标记                    */
  while (1)
  {
    if (time > 286)
      break;
  }
  rotatePID.sRef = 0;
  mouseState = _GoAhead;

  // MouseStop();
  //while(1);
  leftMotor.uiPulse = 2610; //14000
  leftMotor.uiPulseCtr = 0;
  rightMotor.uiPulse = 2610;
  rightMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  mouseState = _TurnLeft;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 10000;
  leftMotor.uiPulseCtr = 10000;
  //MouseStop();
  // while(1);
}

void MouseStop()
{
  mouseState = _Stop;
  maxSpeed = 0;
  AcceleratedSpeed = 0;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  speedPID.sRef = 0;
  rotatePID.sRef = 0;
  rightMotor.uiPulse = 0;
  rightMotor.uiPulseCtr = 0;
  leftMotor.uiPulse = 0;
  leftMotor.uiPulseCtr = 0;
  __PIDControl();
}

void OneStep(void)
{
  speedPID.sRef = 100;
  rightMotor.uiPulse = 20000;
  rightMotor.uiPulseCtr = 0;
  leftMotor.uiPulse = 20000;
  leftMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
}

void MouseTurnBack(void) //搜索掉头用，有后退
{

  if (GucFrontNear)
  {
    speedPID.sRef = 40;
    while ((irValues[_IR_FrontLeft] < GusDistance_FL_Near) || (irValues[_IR_FrontRight] < GusDistance_FR_Near))
      ;
    mouseState = _GoAhead;
    rightMotor.uiPulse = 1000;
    rightMotor.uiPulseCtr = 0;
    leftMotor.uiPulse = 1000;
    leftMotor.uiPulseCtr = 0;
    while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
      ;
    while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
      ;
  }
  backflag = 0;
  if ((irValues[_IR_Left] > GusDistance_L_Near)) //偏左
  {
    backflag = 1;
  }
  GucFrontNear = false;

  speedPID.sRef = 0;
  MouseStop();
  time = 0;

  mouseState = _TurnBack;
  leftMotor.cState = _MotorRun;
  rightMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Back);

  while (1)
  {
    if (time > 870) //掉头角度调节
      break;
  }
  rotatePID.sRef = 0;
  MouseStop();
  mouseState = _GoBack;
  speedPID.sRef = -20;
  rightMotor.uiPulse = 15000; //150
  rightMotor.uiPulseCtr = 0;
  leftMotor.uiPulse = 15000;
  leftMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;

  MouseStop();
  mouseState = _GoAhead;
  Delay(1500000);

  speedPID.sRef = 80;
  mouseState = _GoAhead;
  // MouseStop();
  //while(1);
  rightMotor.uiPulse = 35000; //掉头之后走的距离调节
  rightMotor.uiPulseCtr = 0;
  leftMotor.uiPulse = 35000; //掉头之后走的距离调节
  leftMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;

  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 32000; //32000
  leftMotor.uiPulseCtr = 32000;  //32000
                                // MouseStop();
                                //   while(1);
}

/*********************************************************************************************************
** Function name:       MouseTurnBack_Y
** Descriptions:        原地旋转180度
** input parameters:    无
** output parameters:   无
** Returned value:      无
********************************** **********************************************************************/
void MouseTurnBack_Y(void) //原地掉头不动程序
{

  speedPID.sRef = 0;
  MouseStop();
  Delay(1500000);

  time = 0;
  mouseState = _TurnBack;

  leftMotor.cState = _MotorRun;
  rightMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Back);
  while (1)
  {
    if (time > 850) //掉头角度调节
      break;
  }
  rotatePID.sRef = 0;
  mouseState = _GoAhead;
  MouseStop();
  Delay(1500000);

  rotatePID.sRef = 0;

  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 0;
  leftMotor.uiPulseCtr = 0;

  //  MouseStop();
  //    while(1);
}
void MouseTurnBack_QiDian(void) //终点掉头用，有后退
{

  speedPID.sRef = 0;
  MouseStop();
  Delay(1500000);

  time = 0;
  mouseState = _TurnBack;
  leftMotor.cState = _MotorRun;
  rightMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Back);
  while (1)
  {
    if (time > 750) //掉头角度调节750
      break;
  }
  rotatePID.sRef = 0;
  mouseState = _GoBack;
  speedPID.sRef = -50;       //-50
  rightMotor.uiPulse = 16500; //后退距离，为了矫正 16500
  rightMotor.uiPulseCtr = 0;
  leftMotor.uiPulse = 16500; //后退距离，为了矫正   16500
  leftMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  mouseState = _GoAhead;
  MouseStop();
  Delay(6000000); //等待时间
  rotatePID.sRef = 0;
  speedPID.sRef = 50;
  rightMotor.uiPulse = 16500; //前进距离，为了矫正
  rightMotor.uiPulseCtr = 0;
  leftMotor.uiPulse = 16500; //前进距离，为了矫正
  leftMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;

  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 0;
  leftMotor.uiPulseCtr = 0;
}

void mazeInfDebug()
{
  static char dirChars[4] =
      {
          0x47, /*向前，用F表示*/
          0x77, /*向右，用R表示*/
          0x1f, /*向后，用b表示*/
          0x0e  /*向左，用L表示*/
      };

  /*显示方向*/
  zlg7289Download(2, 3, 0, dirChars[MouseDir]);

  /*显示坐标*/
  zlg7289Download(1, 0, 0, MouseLocal.x / 10);
  zlg7289Download(1, 1, 0, MouseLocal.x % 10);
  zlg7289Download(1, 6, 0, MouseLocal.y / 10);
  zlg7289Download(1, 7, 0, MouseLocal.y % 10);
}

// 标记地图在方向上是否有路
uint8 markWayOnGrid(uint8 map, _Map_Dir dir, _Bool isExist)
{
  return isExist ? map | DIR_MASK(dir) : map & ~DIR_MASK(dir);
}

_Bool IsExistWay(_Map map, _Map_Coor this, _Map_Dir dir)
{
  return map[this.x][this.y] & DIR_MASK(dir) ? true : false;
}

// 将该格四周格子墙壁资料更改
void setGridRoundWay(_Map map, _Map_Coor this)
{
  static _Map_Coor d[4] = {{0, 1}, {1, 0}, {0, -1}, {-1, 0}};
  _Map_Coor o;

  for (_Map_Dir dir = _North; dir <= _West; dir++)
  {
    o.x = this.x + d[dir].x;
    o.y = this.y + d[dir].y;
    if (o.x > 0 && o.x < MAZETYPE && o.y > 0 && o.y < MAZETYPE)
    {
      map[o.x][o.y] = markWayOnGrid(map[o.x][o.y], TurnDir(dir, _Back), IsExistWay(map, this, dir));
    }
  }
}

/*********************************************************************************************************
** Function name:       wallCheck
** Descriptions:        根据传感器检测结果判断是否存在墙壁
** input parameters:    无
** output parameters:   无
** Returned value:      cValue: 低三位从左到右一次代表左前右。1为有墙，0为没墙。
*********************************************************************************************************/
void wallCheck()
{
  uint8 grid;
  uint8 zlg7289View = 0;

  // 记录四周道路
  grid = markWayOnGrid(0, TurnDir(MouseDir, _Back), false);
  grid = markWayOnGrid(grid, MouseDir, !(within(_IR_FrontLeft, _Far) && within(_IR_FrontRight, _Far)));
  grid = markWayOnGrid(grid, TurnDir(MouseDir, _Left), !within(_IR_Left, _Far));
  grid = markWayOnGrid(grid, TurnDir(MouseDir, _Right), !within(_IR_Right, _Far));

  // 保存地图
  GucMapBlock0[MouseLocal.x][MouseLocal.y] = grid;
  GucMapBlock[MouseLocal.x][MouseLocal.y] = grid;
  GucMapBlock1[MouseLocal.x][MouseLocal.y] = grid;

  // 将该格四周格子墙壁资料更改
  setGridRoundWay(GucMapBlock1, MouseLocal);
  setGridRoundWay(GucMapBlock, MouseLocal);

  // 调试器显示障碍
  zlg7289View |= IsExistWay(GucMapBlock, MouseLocal, _North) ? 0 : 0x40;
  zlg7289View |= IsExistWay(GucMapBlock, MouseLocal, _West) ? 0 : 0x06;
  zlg7289View |= IsExistWay(GucMapBlock, MouseLocal, _East) ? 0 : 0x30;
  zlg7289Download(2, 2, 0, zlg7289View);
}

/*********************************************************************************************************
** Function name:       updateMouseCoor
** Descriptions:        根据当前方向更新坐标值
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void updateMouseCoor(void)
{

  switch (MouseDir)
  {

  case _North:
    MouseLocal.y++;
    break;

  case _East:
    MouseLocal.x++;
    break;

  case _West:
    MouseLocal.y--;
    break;

  case _South:
    MouseLocal.x--;
    break;

  default:
    break;
  }

  mazeInfDebug();
  wallCheck();
}

/*********************************************************************************************************
** Function name:       MazeSearch
** Descriptions:        前进N格
** input parameters:    iNblock: 前进的格数
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void MazeSearch(void) //电脑鼠搜索程序
{
  _Bool cL, cR, cCoor;
  uint16 pusle_LR = 0;

  cCoor = leftMotor.cState == _MotorStop; // 若为静止状态下，允许更新坐标

  // 之前状态
  if ((mouseState == _TurnRight) || (mouseState == _TurnLeft))
  {
    leftMotor.uiPulseCtr = 30000;
    rightMotor.uiPulseCtr = 30000;
    cL = true; // 允许探索左边
    cR = true; // 允许探索右边

    if (within(_IR_Front, _Far) || !within(_IR_Left, _Far) || !within(_IR_Right, _Far))
    {
      /* 此时，前方有墙。或者前方无墙，同时，左边无墙，或者右边无墙 */

      // 前方有墙时取 14000；否则，或左或右无墙时取 12000
      pusle_LR = within(_IR_Front, _Far) ? 14000 : 12000;
    }
  }
  else
  {
    /* 之前状态，不向左右转弯，因此也就禁止探索左右 */
    cL = false; // 禁止探索左边
    cR = false; // 禁止探索右边
  }

  mouseState = _GoAhead;
  maxSpeed = SEARCHSPEED;
  rightMotor.uiPulse = MAZETYPE * ONEBLOCK;
  leftMotor.uiPulse = MAZETYPE * ONEBLOCK;
  rightMotor.cState = _MotorRun;
  leftMotor.cState = _MotorRun;
  while (leftMotor.cState != _MotorStop)
  {

    if (leftMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格*/
      leftMotor.uiPulse -= ONEBLOCK;
      leftMotor.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        if (within(_IR_Front, _Far) && within(_IR_Left, _Far) && within(_IR_Right, _Far))
        {
          /* 前、左、右都是墙，死胡同 */

          GucFrontNear = true;
          updateMouseCoor(); /*  更新坐标                    */
          return;
        }
        updateMouseCoor(); /*  更新坐标                    */
      }
      else
      {
        cCoor = true;
      }
    }
    if (rightMotor.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      rightMotor.uiPulse -= ONEBLOCK;
      rightMotor.uiPulseCtr -= ONEBLOCK;
    }

    if (cL)
    { //搜索左转之间调试                                                    /*  是否允许检测左边            */
      if (!within(_IR_Left, _Far))
      { /*  左边有支路，跳出程序        */

        rightMotor.uiPulse = rightMotor.uiPulseCtr + 14100 - pusle_LR; //左转之前调节转弯时距离是否居中
        leftMotor.uiPulse = leftMotor.uiPulseCtr + 14100 - pusle_LR;   //左转之前调节转弯时距离是否居中15500
        while (!within(_IR_Left, _Far))
        {

          if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
          {
            updateMouseCoor(); /*  更新坐标                    */
            return;
          }
        }
        rightMotor.uiPulse = MAZETYPE * ONEBLOCK;
        leftMotor.uiPulse = MAZETYPE * ONEBLOCK;
      }
    }
    else
    { /*  左边有墙时开始允许检测左边  */
      if (within(_IR_Left, _Far))
      {
        cL = true;
      }
    }
    if (cR)
    { //搜索右转之间调试                                                       /*  是否允许检测右边            */
      if (!within(_IR_Right, _Far))
      { /*  右边有支路，跳出程序        */

        rightMotor.uiPulse = rightMotor.uiPulseCtr + 13800 - pusle_LR; //右转之前调节转弯时距离是否居中14100
        leftMotor.uiPulse = leftMotor.uiPulseCtr + 13800 - pusle_LR;   //右转之前调节转弯时距离是否居中
        while (!within(_IR_Right, _Far))
        {

          if ((leftMotor.uiPulseCtr + 100) > leftMotor.uiPulse)
          {
            updateMouseCoor(); /*  更新坐标                    */
            return;
          }
        }
        rightMotor.uiPulse = MAZETYPE * ONEBLOCK;
        leftMotor.uiPulse = MAZETYPE * ONEBLOCK;
      }
    }
    else
    {
      if (within(_IR_Right, _Far))
      {
        cR = true;
      }
    }
  }
  updateMouseCoor(); /*  更新坐标                    */
}

void Go2()
{
  mouseState = _GoAhead;
  maxSpeed = SEARCHSPEED;
  AcceleratedSpeed = 1;
  rightMotor.uiPulse = MAZETYPE * ONEBLOCK;
  leftMotor.uiPulse = MAZETYPE * ONEBLOCK;
  rightMotor.cState = _MotorRun;
  leftMotor.cState = _MotorRun;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
}

/****************************************************************************************************
** Function name:       MouseTurnRight
** Descriptions:        右转
** input parameters:    无
** output parameters:   无
** Returned value:      无         按步数转弯
*********************************************************************************************************/
void MouseTurnRight(void) //电脑鼠搜索右转程序
{

  time = 0;
  rotatePID.sRef = -60; //转弯角速度
  speedPID.sRef = 110; //转弯直线速度
  mouseState = _TurnRight;
  rightMotor.cState = _MotorRun; //得加电脑鼠状态
  leftMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Right); /*  方向标记                    */
  while (1)
  {
    if (time > 270) //转弯角度调节，需把之后距离加大，去掉挡板来看280
      break;
  }

  rotatePID.sRef = 0;
  mouseState = _GoAhead;
  leftMotor.uiPulse = 6200; //转弯之后的距离7000
  leftMotor.uiPulseCtr = 0;
  rightMotor.uiPulse = 6200; //转弯之后的距离  7000
  rightMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  mouseState = _TurnRight;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 10000;
  leftMotor.uiPulseCtr = 10000;
  //MouseStop();
  //while(1);
}
/*********************************************************************************************************
** Function name:       MouseTurnLeft
** Descriptions:        左转
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void MouseTurnLeft(void) //电脑鼠搜索左转程序
{

  time = 0;
  rotatePID.sRef = 60;  //转弯角速度
  speedPID.sRef = 110; //转弯直线速度
  mouseState = _TurnLeft;
  rightMotor.cState = _MotorRun; //得加电脑鼠状态
  leftMotor.cState = _MotorRun;
  MouseDir = TurnDir(MouseDir, _Left); /*  方向标记                    */
  while (1)
  {
    if (time > 285) //转弯角度调节，需把之后距离加大，去掉挡板来看280
      break;
  }
  rotatePID.sRef = 0;
  mouseState = _GoAhead;
  leftMotor.uiPulse = 6000; //转弯之后的距离  7000
  leftMotor.uiPulseCtr = 0;
  rightMotor.uiPulse = 6000; //转弯之后的距离
  rightMotor.uiPulseCtr = 0;
  while ((rightMotor.uiPulseCtr + 200) <= rightMotor.uiPulse)
    ;
  while ((leftMotor.uiPulseCtr + 200) <= leftMotor.uiPulse)
    ;
  mouseState = _TurnLeft;
  rightMotor.cState = _MotorStop;
  leftMotor.cState = _MotorStop;
  rightMotor.sSpeed = 0;
  __RightMotorControl(rightMotor.sSpeed);
  leftMotor.sSpeed = 0;
  __LeftMotorControl(leftMotor.sSpeed);
  rightMotor.uiPulseCtr = 10000;
  leftMotor.uiPulseCtr = 10000;
  //MouseStop();
  // while(1);
}


void EXTI3_IRQHandler(void)
{
  char key;
  if (EXTI_GetITStatus(EXTI_Line3) != RESET)
  {
    key = zlg7289Key();
    if (key != 0xff)
    {
      if (key >= 4)
      {
        if (key == 10)
        {
          DIS[0] = GusDistance_L_Near;
          DIS[1] = GusDistance_R_Near;
          DIS[2] = GusDistance_L_Mid;
          DIS[3] = GusDistance_R_Mid;
          DIS[4] = GusDistance_L_Far;
          DIS[5] = GusDistance_R_Far;
          DIS[6] = GusDistance_FL_Far;
          DIS[7] = GusDistance_FR_Far;
          DIS[8] = GusDistance_FL_Near;
          DIS[9] = GusDistance_FR_Near;
          STMFLASH_Write(IR_SAVE_ADDRESS, (u16 *)DIS, 10);
          Delay(1000000);
        }

        else
        {
          Tab = key;
          ir_flag = 1;
        }
      }
      switch (Tab)
      {
      case 4:
        if (ir_flag == 1)
        {
          GusDistance_L_Near = irValues[_IR_Left];
          GusDistance_R_Near = irValues[_IR_Right];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_L_Near += 10;
          zlg7289Download(0, 4, 0, GusDistance_L_Near / 1000);
          zlg7289Download(0, 5, 0, GusDistance_L_Near / 100 % 10);
          zlg7289Download(0, 6, 0, GusDistance_L_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Near % 10);
        }
        else if (key == 3)
        {
          GusDistance_L_Near -= 1;
          zlg7289Download(0, 5, 0, GusDistance_L_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Near % 10);
        }
        else if (key == 0)
        {
          GusDistance_R_Near += 10;
          zlg7289Download(0, 5, 0, GusDistance_R_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Near % 10);
        }
        else if (key == 2)
        {
          GusDistance_R_Near -= 1;
          zlg7289Download(0, 5, 0, GusDistance_R_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Near % 10);
        }
        break;
      case 5:
        if (ir_flag == 1)
        {
          GusDistance_L_Mid = irValues[_IR_Left];
          GusDistance_R_Mid = irValues[_IR_Right];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_L_Mid += 10;
          zlg7289Download(0, 5, 0, GusDistance_L_Mid / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Mid / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Mid % 10);
        }
        else if (key == 3)
        {
          GusDistance_L_Mid -= 1;
          zlg7289Download(0, 5, 0, GusDistance_L_Mid / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Mid / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Mid % 10);
        }
        else if (key == 0)
        {
          GusDistance_R_Mid += 10;
          zlg7289Download(0, 5, 0, GusDistance_R_Mid / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Mid / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Mid % 10);
        }
        else if (key == 2)
        {
          GusDistance_R_Mid -= 1;
          zlg7289Download(0, 5, 0, GusDistance_R_Mid / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Mid / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Mid % 10);
        }
        break;
      case 6:
        if (ir_flag == 1)
        {
          GusDistance_L_Far = irValues[_IR_Left];
          GusDistance_R_Far = irValues[_IR_Right];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_L_Far += 10;
          zlg7289Download(0, 5, 0, GusDistance_L_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Far % 10);
        }
        else if (key == 3)
        {
          GusDistance_L_Far -= 1;
          zlg7289Download(0, 5, 0, GusDistance_L_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Far % 10);
        }
        else if (key == 0)
        {
          GusDistance_R_Far += 10;
          zlg7289Download(0, 5, 0, GusDistance_R_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Far % 10);
        }
        else if (key == 2)
        {
          GusDistance_R_Far -= 1;
          zlg7289Download(0, 5, 0, GusDistance_R_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Far % 10);
        }
        break;
      case 7:
        if (ir_flag == 1)
        {
          GusDistance_FL_Far = irValues[_IR_FrontLeft];
          GusDistance_FR_Far = irValues[_IR_FrontRight];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_FL_Far += 10;
          zlg7289Download(0, 5, 0, GusDistance_FL_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_FL_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FL_Far % 10);
        }
        else if (key == 3)
        {
          GusDistance_FL_Far -= 1;
          zlg7289Download(0, 5, 0, GusDistance_FL_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_FL_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FL_Far % 10);
        }
        else if (key == 0)
        {
          GusDistance_FR_Far += 10;
          zlg7289Download(0, 5, 0, GusDistance_FR_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_FR_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FR_Far % 10);
        }
        else if (key == 2)
        {
          GusDistance_FR_Far -= 1;
          zlg7289Download(0, 5, 0, GusDistance_FR_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_FR_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FR_Far % 10);
        }
        break;
      case 8:
        if (ir_flag == 1)
        {
          GusDistance_FL_Near = irValues[_IR_FrontLeft];
          GusDistance_FR_Near = irValues[_IR_FrontRight];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_FL_Near += 10;
          zlg7289Download(0, 5, 0, GusDistance_FL_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_FL_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FL_Near % 10);
        }
        else if (key == 3)
        {
          GusDistance_FL_Near -= 1;
          zlg7289Download(0, 5, 0, GusDistance_FL_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_FL_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FL_Near % 10);
        }
        else if (key == 0)
        {
          GusDistance_FR_Near += 10;
          zlg7289Download(0, 5, 0, GusDistance_FR_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_FR_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FR_Near % 10);
        }
        else if (key == 2)
        {
          GusDistance_FR_Near -= 1;
          zlg7289Download(0, 5, 0, GusDistance_FR_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_FR_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FR_Near % 10);
        }
        break;

      default:
        break;
      }
      Delay(200000);
      EXTI_ClearITPendingBit(EXTI_Line3);
    }
  }
}